#ifndef DSO_COARSE_TRACKER_H
#define DSO_COARSE_TRACKER_H

#include <vector>
#include <cmath>

#include "dso/util/NumType.h"
#include "dso/util/Settings.h"
#include "dso/OptimizationBackend/MatrixAccumulators.h"
#include "dso/IOWrapper/Output3DWrapper.h"

namespace dso
{
struct CalibHessian;
struct FrameHessian;
struct PointFrameResidual;

// the coarse tracker is used to estimate the coarse pose of current frame
class CoarseTracker
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    // constuctor, allocate memory and compute the camera intrinsics pyramid
    CoarseTracker ( int w, int h );
    ~CoarseTracker();

    /** @brief track the new coming frame and estimate its pose and light parameters
     * @param[in] newFrameHessian the new frame
     * @param[out] lastToNew_out T_new_last
     * @param[out] aff_g2l_out affine transform
     * @param[in] coarsestLvl the first pyramid level (default=5)
     * @param[in] minResForAbort if residual > 1.5* minResForAbort, return false
     * @return true if track is good
     */
    bool trackNewestCoarse (
        FrameHessian* newFrameHessian,
        SE3 &lastToNew_out, AffLight &aff_g2l_out,
        int coarsestLvl, Vec5 minResForAbort,
        IOWrap::Output3DWrapper* wrap=0 );

    void setCoarseTrackingRef (
        std::vector<FrameHessian*> frameHessians );

    // create camera intrinsics in each level
    void makeK ( CalibHessian* HCalib );


    FrameHessian* lastRef =nullptr;     // the reference frame
    AffLight lastRef_aff_g2l;
    // 新来的帧
    FrameHessian* newFrame =nullptr;    // the new coming frame
    int refFrameID =-1;

    // act as pure ouptut
    Vec5 lastResiduals;
    Vec3 lastFlowIndicators;
    double firstCoarseRMSE =0;

    // camera and image parameters in each pyramid
    // 每个金字塔的参数
    Mat33f K[PYR_LEVELS];
    Mat33f Ki[PYR_LEVELS];
    float fx[PYR_LEVELS];
    float fy[PYR_LEVELS];
    float fxi[PYR_LEVELS];
    float fyi[PYR_LEVELS];
    float cx[PYR_LEVELS];
    float cy[PYR_LEVELS];
    float cxi[PYR_LEVELS];
    float cyi[PYR_LEVELS];
    int w[PYR_LEVELS];
    int h[PYR_LEVELS];

    // debug only
    void debugPlotIDepthMap ( float* minID, float* maxID, std::vector<IOWrap::Output3DWrapper*> &wraps );
    void debugPlotIDepthMapFloat ( std::vector<IOWrap::Output3DWrapper*> &wraps );
    bool debugPrint =true, debugPlot =true;

private:

    void makeCoarseDepthL0 ( std::vector<FrameHessian*> frameHessians );

    // unimplemented
    Vec6 calcResAndGS ( int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, AffLight aff_g2l, float cutoffTH );

    // 计算残差
    // compute the residual
    /**
     * @param[in] lvl the pyramid level
     * @param[in] refToNew pose from reference to current
     * @param[in] aff_g2l affine light transform from g to l
     * @param[in] cutoffTH cut off threshold, if residual > cutoffTH, then make residual = max energy. Similar with robust kernel in g2o.
     * @return the residual vector (a bit complicated, the the last lines in this func.)
     */
    Vec6 calcRes ( int lvl, const SE3 &refToNew, AffLight aff_g2l, float cutoffTH );


    // unimplemented
    void calcGS ( int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, AffLight aff_g2l );

    // SSE 版本的Gauss-Newton(or L-M because we have lambda in tracking)
    /** @brief SSE accelerated Gauss-Newton
     * NOTE it uses some cache data in "warped buffers"
     * @param[in] lvl image pyramid level
     * @param[out] H_out Hessian matrix
     * @param[out] b_out bias vector
     * @param[in] refToNew Transform matrix from ref to new
     * @param[in] aff_g2l affine light transform
     */
    void calcGSSSE ( int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, AffLight aff_g2l );

    // pc buffers
    // 每一层都是w*h的矩阵，w,h为该层金字塔的分辨率
    float* pc_u[PYR_LEVELS];            // x 坐标
    float* pc_v[PYR_LEVELS];            // y 坐标
    float* pc_idepth[PYR_LEVELS];       // 参考帧的 inv depth
    float* pc_color[PYR_LEVELS];
    int pc_n[PYR_LEVELS];               // 每层pyramid的点数量

    // warped buffers
    // 缓存用的buffer, w*h 那么大，也可以看成图像
    float* buf_warped_idepth;
    float* buf_warped_u;
    float* buf_warped_v;
    float* buf_warped_dx;
    float* buf_warped_dy;
    float* buf_warped_residual;
    float* buf_warped_weight;
    float* buf_warped_refColor;
    int buf_warped_n;

    float* idepth[PYR_LEVELS];
    float* weightSums[PYR_LEVELS];
    float* weightSums_bak[PYR_LEVELS];

    std::vector<float*> ptrToDelete;    // all allocated memory, will be deleted in deconstructor
    Accumulator9 acc;
};


// the distance map
class CoarseDistanceMap
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    CoarseDistanceMap ( int w, int h );
    ~CoarseDistanceMap();

    void makeDistanceMap (
        std::vector<FrameHessian*> frameHessians,
        FrameHessian* frame );

    void makeInlierVotes (
        std::vector<FrameHessian*> frameHessians );

    void makeK ( CalibHessian* HCalib );


    float* fwdWarpedIDDistFinal;

    Mat33f K[PYR_LEVELS];
    Mat33f Ki[PYR_LEVELS];
    float fx[PYR_LEVELS];
    float fy[PYR_LEVELS];
    float fxi[PYR_LEVELS];
    float fyi[PYR_LEVELS];
    float cx[PYR_LEVELS];
    float cy[PYR_LEVELS];
    float cxi[PYR_LEVELS];
    float cyi[PYR_LEVELS];
    int w[PYR_LEVELS];
    int h[PYR_LEVELS];

    void addIntoDistFinal ( int u, int v );


private:

    PointFrameResidual** coarseProjectionGrid;
    int* coarseProjectionGridNum;
    Eigen::Vector2i* bfsList1;
    Eigen::Vector2i* bfsList2;

    void growDistBFS ( int bfsNum );
};


}

#endif
